clear ;
clc;
theta(1) = 0; d(1) = 90; a(1) = 0; alp(1) = pi/2;
theta(2) = 0; d(2) = 0; a(2) = 355; alp(2) = 0;   
theta(3) = 0; d(3) = 0; a(3) = 0; alp(3) = pi/2;
theta(4) = 0; d(4) = 396; a(4) = 0; alp(4) = -pi/2;
theta(5) = 0; d(5) = 0; a(5) = 0; alp(5) = pi/2;
theta(6) = 0; d(6) = 70; a(6) = 0; alp(6) = 0;

% DH parameters  th     d    a    alpha  sigma
L1 = Link([theta(1), d(1), a(1), alp(1), 0]);
L2 = Link([theta(2), d(2), a(2), alp(2), 0]);
L3 = Link([theta(3), d(3), a(3), alp(3), 0]);
L4 = Link([theta(4), d(4), a(4), alp(4), 0]);
L5 = Link([theta(5), d(5), a(5), alp(5), 0]);  
L6 = Link([theta(6), d(6), a(6), alp(6), 0]);

robot = SerialLink([L1, L2, L3, L4, L5, L6]); 
robot.name='赵海生6轴';
robot.display() 
theta = [0, 0, 0, 0, 0,0]*pi/180;
robot.teach();
robot.plot(theta); 
t = robot.fkine(theta) ;   %末端执行器位姿


% joint_limit
L1.qlim=[-180,180]/180*pi;
L2.qlim=[-180,180]/180*pi;
L3.qlim=[-180,180]/180*pi;
L4.qlim=[-180,180]/180*pi;
L5.qlim=[-180,180]/180*pi;
L6.qlim=[-180,180]/180*pi;
%关节工作空间

